package edu.csusm.spotrobot;

import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IIOPin;
import com.sun.spot.sensorboard.peripheral.Servo;

/**
 *
 * Controls the PING ultrasonic ranger from Parallax.
 *
 * @author Clark Scheff
 * @author Zebulon Evans
 */
public class SonarSensor {
    
    private EDemoBoard demo = EDemoBoard.getInstance();
    private float lastReading = -1;
    private IIOPin sonarPin;
    
    /**
     * Create a new SonarSensor
     *
     * @param int the pin that the sonar is connected to on the SunSPOT
     */
    public SonarSensor( int sonarPinIn ){
        sonarPin = demo.getIOPins()[sonarPinIn];
    }
    
    /**
     * Gets a reading from the sonar
     *
     * @return float representing the distance in cm
     */
    public float getReading(){
        float reading1 = 0;
        float reading2 = 0;
        float reading3 = 0;
        try{
        while( reading1 <= 0 || reading2 <= 0 || reading3 <= 0 ){
            System.out.println("Pinging");
           demo.startPulse(sonarPin, true, 5);
           int ptime = demo.getPulse(sonarPin, true,500);

           reading1 = (float)(ptime / 58.309038);
           Thread.sleep(10);
           
           demo.startPulse(sonarPin, true, 5);
           ptime = demo.getPulse(sonarPin, true,500);

           reading2 = (float)(ptime / 58.309038);
           Thread.sleep(10);
           demo.startPulse(sonarPin, true, 5);
           ptime = demo.getPulse(sonarPin, true,500);

           reading3 = (float)(ptime / 58.309038);
           
           if( Math.abs( reading1 - reading2 ) > 2.0f || Math.abs( reading2 - reading3 ) > 2.0f ){
               reading1 = 0;
               reading2 = 0;
               reading3 = 0;
               System.out.println("Repinging because we did not get good readings");
               continue;
           }
        }
        }catch(Exception e){}
        return (reading1 + reading2 + reading3) / 3.0f;
         
    }
    

}
